#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <vxWorks.h>
#include <intLib.h>
#include <logLib.h>
#include <semLib.h>
#include <sysLib.h>
#include <vxBusLib.h>
#include <hwif/vxbus/vxBus.h>
#include <hwif/vxbus/hwConf.h>
#include <hwif/vxbus/vxbPciLib.h>
#include <hwif/util/vxbDmaBufLib.h>
#include <hwif/util/vxbParamSys.h>
#include <../src/hwif/h/vxbus/vxbAccess.h>
#include <drv/pci/pciConfigLib.h>
#include <drv/pci/pciExpressLib.h>
#include "../hardWare.h"

#define IO_OUT_MAX_NUM    64
#define IO_OUT_MAX_CHANNEL    64

#define IO_OUT_DEBUG

#ifdef IO_OUT_DEBUG
int fpgaIoOutDebugFlag = 0;
#define IO_OUT_DEBUG_MSG(...) {if(fpgaIoOutDebugFlag) printf(__VA_ARGS__);}
#else
#define IO_OUT_DEBUG_MSG(...)
#endif

#define IO_OUT_DEVICE_OFFSET(dev)  (0x70000 + (0x40*(dev)))
#define IO_OUT_CHANNEL_OFFSET(ch)  ((ch/32)*0x4 + 0x0)
#define FPGA_NUM   2

extern int gAbSwitchFlag;
extern int gAbDevNum;
static addr_t ioOutBaseAddr[FPGA_NUM][IO_OUT_MAX_NUM];

extern void fpgaRegister(void);

int IoOutInit(void)
{
	VXB_DEVICE_ID fpga;
	int i;
	int fpgaNum;
	static int initFlag = 0;

	/*fpgaRegister();*/
	if (initFlag == 0)
	{
		for (fpgaNum = 0; fpgaNum < gAbDevNum; fpgaNum++)
		{
			fpga = vxbInstByNameFind("pcieFpga", fpgaNum);
			if (fpga == NULL )
			{
				IO_OUT_DEBUG_MSG(
						"Error @IoOutInit can not found device '%s%d'\n", "pcieFpga", fpgaNum);
				return ERROR;
			}

			for (i = 0; i < IO_OUT_MAX_NUM; i++)
			{
				ioOutBaseAddr[fpgaNum][i] = fpga->pRegBase[0]
						+ IO_OUT_DEVICE_OFFSET(i);
				IO_OUT_DEBUG_MSG(
						"IoOutInit: ioOutBaseAddr[%d][%d] = %p\n", fpgaNum, i, ioOutBaseAddr[fpgaNum][i]);
			}
		}
	}
	initFlag = 1;

	return OK;
}

int IoOutWriteIO(int devNum, int channel[], unsigned char io[], int n)
{
	int fpgaNum = gAbSwitchFlag;
	unsigned int data[2];
	int i;
	char flag[2] = {0,0};

	if ((devNum >= IO_OUT_MAX_NUM) /*|| (channel >= IO_OUT_MAX_CHANNEL)*/)
	{
		IO_OUT_DEBUG_MSG(
				"Error @IoOutSet10V devNum(%d) \n", devNum);
		return ERROR;
	}
	
	for(i = 0; i < n; i++)
	{
		if(channel[i] < 32)
			flag[0] = 1;
		else
			flag[1] = 1;
		
		if((flag[0] == 1)&&(flag[1] == 1))
			break;
	}

	for (fpgaNum = 0; fpgaNum < gAbDevNum; fpgaNum++)
	{
		if(flag[0] == 1)
		{
			data[0] = *(volatile unsigned int *) (ioOutBaseAddr[fpgaNum][devNum]
					+ IO_OUT_CHANNEL_OFFSET(0));
		}
		if(flag[1] == 1)
		{
			data[1] = *(volatile unsigned int *) (ioOutBaseAddr[fpgaNum][devNum]
					+ IO_OUT_CHANNEL_OFFSET(32));
		}
		for(i = 0; i < n; i++)
		{
			if (io[i])
			{
				data[(channel[i]>=32) ? 1:0] |= (1 << (channel[i] &0x1f));
			}
			else
			{
				data[(channel[i]>=32) ? 1:0] &= (~(1 << (channel[i] &0x1f)));
			}
			IO_OUT_DEBUG_MSG(
					"IoOutWriteIO:dev[%d] ch[%d] = %d\n", devNum, channel[i], io[i]);
		}
		if(flag[0] == 1)
		{
			*(volatile unsigned int *) (ioOutBaseAddr[fpgaNum][devNum]
					+ IO_OUT_CHANNEL_OFFSET(0)) = data[0];
		}
		if(flag[1] == 1)
		{
			*(volatile unsigned int *) (ioOutBaseAddr[fpgaNum][devNum]
					+ IO_OUT_CHANNEL_OFFSET(32)) = data[1];
		}
	}

	return OK;
}


int IoOutWriteIO2(int devNum, unsigned int io[])
{
	int fpgaNum = gAbSwitchFlag;

	if ((devNum >= IO_OUT_MAX_NUM) /*|| (channel >= IO_OUT_MAX_CHANNEL)*/)
	{
		IO_OUT_DEBUG_MSG(
				"Error @IoOutSet10V devNum(%d) \n", devNum);
		return ERROR;
	}

	for (fpgaNum = 0; fpgaNum < gAbDevNum; fpgaNum++)
	{
		*(volatile unsigned int *) (ioOutBaseAddr[fpgaNum][devNum]
					+ IO_OUT_CHANNEL_OFFSET(0)) = io[0];
		*(volatile unsigned int *) (ioOutBaseAddr[fpgaNum][devNum]
					+ IO_OUT_CHANNEL_OFFSET(32)) = io[1];
	}
	IO_OUT_DEBUG_MSG(
					"IoInWriteIO:dev[%d] io1[0x%08X] io2[0x%08X]\n", devNum, io[0], io[1]);
	
	return OK;
}
